// Copyright (C) 2023 Thies Lennart Alff
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301,
// USA

#include <hippo_control/trajectory_tracking_bluerov/tracking_control_bluerov.hpp>

namespace hippo_control {
namespace trajectory_tracking_bluerov {
TrackingControllerBlueROV::TrackingControllerBlueROV() {}

// 修改：不再需要AttitudeFromThrust函数，因为姿态直接由外部提供
// 删除 AttitudeFromThrust 函数

// 修改：Update函数现在只计算推力，返回世界坐标系下的推力向量
Eigen::Vector3d TrackingControllerBlueROV::Update(
    const Eigen::Vector3d &_position, const Eigen::Vector3d &_velocity) {
  position_ = _position;
  velocity_ = _velocity;
  //计算推力
  return ComputeThrust(); 
}

Eigen::Vector3d TrackingControllerBlueROV::ComputeThrust() {
  return Eigen::Vector3d{(position_desired_ - position_) * position_gain_ +
                         (velocity_desired_ - velocity_) * velocity_gain_ };
}
}  // namespace trajectory_tracking
}  // namespace hippo_control
